import csv
import math
import yaml
import numpy as np

# 获取当前csv文件和其有效行数
# infile = input('请输入文件路径：\n')
infile = '/home/fanwei/桌面/SAIC/raw data/object2-test.csv'
n = sum(1 for line in open(infile)) - 1
# 获取输出yaml输出路径和名字
path = __file__
for i in range(len(path) -1, -1, -1):
    if infile[i] == "/":
        l0 = i+1
        break
for i in range(len(infile) -1, -1, -1):
    if infile[i] == "/":
        l1 = i+1
        break
name0 = path[0:l0]
name1 = infile[l1:-9]
name2="-problem frame.yml"
outfile = name0 + name1 + name2 
print(outfile)
#读取csv文件
with open(infile,'rt') as csvfile2:
  reader2 = csv.reader(csvfile2)
  rows2 = [row for row in reader2]
  H2 = np.array(rows2)

  #设置速度阈值
  Tv  =  3
  # 写入初始yaml文件
with open(outfile, "w") as output_stream:
  yaml.dump(rows2[0], output_stream,default_flow_style=True)

  #进行评估并输出问题帧yaml文件
for i in range(1,n+1):
  if abs(float(H2[i,4])) >= Tv or abs(float(H2[i,5])) >= Tv:#作差与阈值比较进行判断
    with open(outfile, "a") as output_stream:
            yaml.dump(rows2[i], output_stream,default_flow_style=True)